# Module Containing Equations of Motion derivatives
import math
import motor
import controller
from numpy import *
#Input Vector Format
#state=[x y z u v w phi theta psi p q r]
#dstate=[xdot ydot zdot udot vdot wdot phidot thetadot psidot pdot qdot rdot]

def eqn_deriv(state,t):
	g=9.81  #m/s^2
	
	#Geometry Definitions
	J=array([1,1,1])
	m=1;
	l=1;
	
	ztarg=10
	
	x = state[0]
	y = state[1]
	z = state[2]
	u = state[3]
	v = state[4]
	w = state[5]
	phi = state[6]
	theta = state[7]
	psi = state[8]
	p = state[9]
	q = state[10]
	r = state[11]
	
	#Define trigonometrics to simplify rotation matrices
	cphi = math.cos(phi)
	sphi = math.sin(phi)
	cth = math.cos(theta)
	sth = math.sin(theta)
	cpsi = math.cos(psi)
	spsi = math.sin(psi)
	tth = math.tan(theta)
	
	#Get Motor info
	Force=controller.control(state,0.01,ztarg,l)
	F=Force[0]
	tauphi=Force[1]
	tauth=Force[2]
	taupsi=Force[3]
	
	
	#Coefficient Matrices
	A = array( [(cth*cpsi, sphi*sth*cpsi-cphi*spsi, cphi*sth*cpsi+sphi*spsi),
				(cth*spsi, sphi*sth*spsi+cphi*cpsi, cphi*sth*spsi-sphi*cpsi),
				(sth,      -sphi*cth,               -cphi*cth)] )
	
	B = array( [(1, sphi*tth, cphi*tth),(0, cphi, -sphi),(0, sphi/cth, cphi/cth)] )
	
	#Calculate Derivatives
	#Velocities(inertial frame)
	V=array( [ u, v, w] )
	pos_dot = dot(A,V)
	xdot=pos_dot[0]
	ydot=pos_dot[1]
	zdot=pos_dot[2]
	
	#Accles (in bodyframe)
	udot=r*v-q*w-g*sth
	vdot=p*w-r*u+g*cth*sphi
	wdot=q*u-p*v+g*cth*cphi-F/m
	
	#Angular Rates (pitch, roll, yaw)
	rr_deriv = array( [ p, q, r])
	Roll_Rate = dot(B,rr_deriv)
	phidot = Roll_Rate[0]
	thdot = Roll_Rate[1]
	psidot = Roll_Rate[2]
	
	#Angular Accels
	pdot = (J[1]-J[2])/J[0]*q*r+tauphi/J[0]
	qdot = (J[2]-J[0])/J[1]*p*r+tauth/J[1]
	rdot = (J[0]-J[1])/J[2]*p*q+taupsi/J[2]
	
	dstate=array( [xdot, ydot, zdot, udot, vdot, wdot, phidot, thdot, psidot, pdot, qdot, rdot] )
	
	return dstate